#include<SoftwareSerial.h>

#define LEDPIN 4
#define TRXRATE 19200
#define TERMBAUD 9600
#define RX_1 6
#define TX_1 7
#define RX_2 8
#define TX_2 9
#define RX_3 10
#define TX_3 11
#define RX_4 12
#define TX_4 13
SoftwareSerial mySerial_1(RX_1,TX_1);
SoftwareSerial mySerial_2(RX_2,TX_2);
SoftwareSerial mySerial_3(RX_3,TX_3);
SoftwareSerial mySerial_4(RX_4,TX_4);

int isFirst;
long rand100;
int collecting_IDs;
uint8_t IDs[16];
uint8_t ID_read[4];
int id_count;
int noNewID = 0;
char msg_buf[16] = "TEST_MESSAGE";
long prevTime;
long time_1;
long time_2;
int ledState = LOW;

void blink_led(int ledPin){
  long blinkTime = 250;
  long curTime = millis();
  if((curTime - prevTime) > blinkTime){
    prevTime = curTime;
    if(ledState == LOW){
      ledState = HIGH;
    }else{
      ledState = LOW;
    }
  }
  digitalWrite(ledPin, ledState);
}

//https://stackoverflow.com/questions/15094834/check-if-a-value-exist-in-a-array
int isvalueinarray(byte val, byte *arr, int size){
    int i;
    for (i=0; i < size; i++) {
        if (arr[i] == val)
            return 1;
    }
    return 0;
}

//Write ID buffer to serial TX
void writebuffertoserial(int channel, uint8_t *arr, int buf_size) {
  int i;
  Serial.print("Writing");
  for(i=0;i<buf_size;i++){
    Serial.print(", ");
    Serial.print(arr[i], DEC);
  }
  Serial.print(" to serial channel ");
  Serial.println(channel, DEC);
  switch(channel){
    case 1:
      mySerial_1.write(arr, buf_size);
      break;
    case 2:
      mySerial_2.write(arr, buf_size);
      break;
    case 3:
      mySerial_3.write(arr, buf_size);
      break;
    case 4:
      mySerial_4.write(arr, buf_size);
      break;
    default:
      Serial.println("Invalid channel selection for writebuffertoserial()");
      Serial.print("Error! cannot write to serial channel ");
      Serial.println(channel);
      break;
    }
}

void checkID(uint8_t ID){
  if(ID > 0 && ID <= 100){
    if(ID > IDs[0] || isFirst != 1){
      digitalWrite(LEDPIN, LOW);
      isFirst = 0;
    }else{
      digitalWrite(LEDPIN, HIGH);
    }
    if(!isvalueinarray(ID, IDs, id_count)){
      IDs[id_count] = ID;
      id_count++;
      Serial.print(ID, DEC);
      Serial.println(" added"); // to the ID list.");
      Serial.print("id_count "); //now equals: ");
      Serial.println(id_count, DEC);
      noNewID = 0;
    }else{
      noNewID++;
      if(noNewID > 25){
        Serial.print(id_count, DEC);
        Serial.println(" IDs found");
        collecting_IDs = 0;
      }
    }
  }
}

void setup(){
  Serial.begin(TERMBAUD);
  Serial.println("attempting to initialize Serial comms...");
  Serial.println("...");
  pinMode(RX_1, INPUT);
  pinMode(TX_1, OUTPUT);
  pinMode(RX_2, INPUT);
  pinMode(TX_2, OUTPUT);
  pinMode(RX_3, INPUT);
  pinMode(TX_3, OUTPUT);
  pinMode(RX_4, INPUT);
  pinMode(TX_4, OUTPUT);
  mySerial_1.begin(TRXRATE);
  Serial.println("...");
  //delay(500);
  mySerial_2.begin(TRXRATE);
  Serial.println("...");
  //mySerial_3.begin(TERMBAUD);
  //Serial.println("...");
  //mySerial_4.begin(TERNBAUD);
  pinMode(LEDPIN, OUTPUT);
  Serial.println("...initialization complete.");
  Serial.println("Generating random ID...");
  randomSeed(analogRead(A5));
  rand100 = random(1,101);
  IDs[0] = uint8_t(rand100);
  //IDs[1] = uint8_t(76);
  id_count = 1;
  collecting_IDs = 1;
  isFirst = 1;
  msg_buf[12] = 0;
  digitalWrite(LEDPIN, HIGH);
  prevTime = millis();
}

void loop(){
  if(collecting_IDs){
    //mySerial_1.write(msg_buf, 12);
    writebuffertoserial(1, IDs, id_count);
    //delay(7);
    writebuffertoserial(2, IDs, id_count);
    delay(1);
    mySerial_1.listen();
    delay(1);
    Serial.print("The next value in the buffer for channel 1 is ");
    Serial.println(mySerial_1.peek());
    while(mySerial_1.available() > 0){
      ID_read[0] = mySerial_1.read();
      //time_1 = millis();
      
      /*Serial.print("Read ");
      Serial.print(ID_read[0], DEC);
      Serial.println(" from serial channel 1.");*/
      
      checkID(ID_read[0]);
      //time_2 = millis();
      //Serial.println(time_2 - time_1, DEC);
    }
    mySerial_2.listen();
    delay(1);
    Serial.print("The next value in the buffer for channel 2 is ");
    Serial.println(mySerial_2.peek(), DEC);
    while(mySerial_2.available() > 0){
      ID_read[1] = mySerial_2.read();
      //time_1 = millis();
      
      /*Serial.print("Read ");
      Serial.print(ID_read[1], DEC);
      Serial.println(" from serial channel 2.");*/
      
      checkID(ID_read[1]);
      //time_2 = millis();
      //Serial.println(time_2 - time_1, DEC);
    }
  }
  if(isFirst == 1){
    blink_led(LEDPIN);
  }else{
    
    digitalWrite(LEDPIN, LOW);
  }
  writebuffertoserial(1, IDs, id_count);
  //delay(7);
  writebuffertoserial(2, IDs, id_count);
  delay(1);
}
